/*
 * @Author: Mcfly
 * @Date: 2021-07-27 11:24:05
 * @LastEditTime: 2021-11-05 19:54:49
 * @LastEditors: Mcfly
 * @Description: 
 * @FilePath: \FTTfly\Ctrl\XY_Ctrl.h
 * 本人github:https://github.com/McflyWZX
 */
#ifndef __XY_CTRL_H_
#define __XY_CTRL_H_

#include "stdint.h"
#include "PID.h"
#include "WP_DataType.h"
#include "Height.h"

typedef struct
{
    PIDctrler PID_X_Pos;
    PIDctrler PID_X_Speed;
    PIDctrler PID_Y_Pos;
    PIDctrler PID_Y_Speed;
    Vector2f lastXY_Speed;
	Vector2f OptPos;
	Vector2f lastXY_AccelSpeed;
	Vector2f lastXY_AccelPos;
    Vector2f outPut;
}XY_Ctrl;

XY_Ctrl *XY_Ctrl_Init(void);
// void XY_Ctrl_UpdateFrame_OnlyMVpos(XY_Ctrl *xyCtrl, Vector2f nowXY, Vector2f expectXY);
// void XY_Ctrl_SpeedUpdateFrame(XY_Ctrl *xyCtrl, Vector2f nowXY_Speed, Vector2f expectXY_Speed, Vector2f nowAngle);
void XY_Ctrl_SpeedUpdateFrame_Single(XY_Ctrl *xyCtrl, Vector2f nowXY_Speed, Vector2f expectXY_Speed, Vector2f nowAccel, Vector2f nowGyro, Height *height);
void XY_Ctrl_PosUpdateFrame(XY_Ctrl *xyCtrl, Vector2f nowXY_Pos, Vector2f expectXY_Pos);
void XY_Ctrl_Clear(XY_Ctrl *xyCtrl);

#endif
